// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "armconfig.h"

// 3.自定义节点类；
class Control_Arm : public rclcpp::Node
{
public:
    Control_Arm() : Node("control_arm_node_cpp")
    {
        // 创建位置指令订阅回调
        this->pos_sub_ = this->create_subscription<std_msgs::msg::Float32>("pos_true", 10, std::bind(&Control_Arm::posCallback, this, std::placeholders::_1));
        this->vel_sub_ = this->create_subscription<std_msgs::msg::Float32>("vel_true", 10, std::bind(&Control_Arm::velCallback, this, std::placeholders::_1));
    }

private:
    // 位置节点订阅
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr pos_sub_;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr vel_sub_;

    void velCallback(const std_msgs::msg::Float32::SharedPtr vel_msg)
    {
        std::cout<<"shuchu"<<std::endl;
        int V_x_1_3 = round(vel_msg->data);
        std::ostringstream stream_1_3;
        stream_1_3 << std::hex << V_x_1_3;
        std::string hexStr_1_3 = stream_1_3.str();
        unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_05[5] = {
            0x1d,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        // 发送数据
        SendData(send_motor_speed, motor_id_05, speed_data_05, 5);
    }
    // 位置订阅回调
    void posCallback(const std_msgs::msg::Float32::SharedPtr pos_msg)
    {
        // 速度控制 500
        BYTE pos_p[5] = {0x24, 0xf4, 0x01, 0x00, 0x00};
        SendData(send_motor_speed, motor_id_05, pos_p, 5);
        // 速度控制 -500
        BYTE pos_r[5] = {0x25, 0x0c, 0xfe, 0xff, 0xff};
        SendData(send_motor_speed, motor_id_05, pos_r, 5);
        // 取负值，保证向右转为正数
        float pos_msg_ = -pos_msg->data;
        int motor_val = pos_val(pos_msg_);
        // 数据转换，机械臂电机
        std::cout << "十进制数: " << motor_val << std::endl;
        std::ostringstream stream_5;
        stream_5 << std::hex << motor_val;
        std::string hexStr_5 = stream_5.str();
        std::cout << "十六进制数: " << hexStr_5 << std::endl;
        // 机械臂位置
        unsigned int num_1 = std::stoul(hexStr_5, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_05[5] = {
            0x1e,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        SendData(send_motor_speed, motor_id_05, speed_data_05, 5);
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 初始化CAN设备
    Init_Can();
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Control_Arm>());
    // 停止电机
    SendData(send_motor_speed, motor_id_05, pos_zero, 5);
    std::this_thread::sleep_for(5s);
    // 速度降低后关闭电机
    SendData(send_motor_speed, motor_id_05, motor_stop, 1);
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}

// CAN设备初始化函数
void Init_Can(void)
{
    // 初始化CAN设备
    device_type = VCI_USBCAN2; // 设备类型修改
    device_index = 0;          // 设备的索引
    can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

    if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
    {
        std::cout << "无法开启CAN设备" << std::endl;
        return;
    }

    if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
    {
        std::cout << "获取设备信息失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
    // 设置CAN初始化配置
    init_config.AccCode = 0X00000000;
    init_config.AccMask = 0xFFFFFFFF;
    init_config.Filter = 2; // 只接受标准帧
    init_config.Mode = 0;
    init_config.Timing0 = 0x00;
    init_config.Timing1 = 0x14; // 波特率：1000kps

    if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
    {
        std::cout << "初始化CAN参数失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }

    // 开始CAN通讯
    if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
    {
        std::cout << "开启CAN通道失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
}

// 指令发送函数
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
    handle_obj.ID = id;
    handle_obj.RemoteFlag = 0;
    handle_obj.ExternFlag = 0;
    handle_obj.DataLen = dataLen;
    for (int i = 0; i < handle_obj.DataLen; i++)
    {
        handle_obj.Data[i] = data[i];
    }
    if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
    {
    }
    else
    {
        std::cout << "初始化错误！" << std::endl;
    }
}

// 位置信息转换

int pos_val(float pos_ang)
{
    int pos_ang_int = round(pos_ang / 360 * 101 * 65536);
    return pos_ang_int;
}
